#ifndef SERVER_
#define SERVER_

#define STDIN 0

#include "include/server.h"

Server* Server::theServer = new Server(SERVER_JOY_FD, SERVER_ARDUI_FD, SERVER_ARDUI_BAUD, PORT_NO);

Server::Server(const char* fd_joystick, const char* fd_ardui, int baud, int portno){
  this->running = true;
  this->fd_joystick = fd_joystick;
  this->fd_ardui = fd_ardui;
  this->baud = baud;
  this->portno = portno;

#ifdef NO_JOY
  joy_x = 128;
  joy_y = 128;
  joy_z = 128;
  joy_but_0 = 0;
  joy_but_1 = 0;
#endif // #ifdef NO_JOY

}

Server::~Server(void){
  delete port_comm;
  delete input_reader;
}

void Server::run(void){
  printf("Entering run of server. Attempting init.\n");
  while(init() < 0){
    fprintf(stderr, "Errors initializing server.\n");
    fprintf(stderr, "Probably a good idea to try again.\n");
    fprintf(stderr, "Reattempting init in 5 sec.\n");
    sleep(5);
  }
  printf("Finished. Now have finished init.\n");

#ifdef NO_JOY
  struct timeval tv;
  fd_set readfd;

  tv.tv_sec = 0;
  tv.tv_usec = 1000;
#endif // #ifdef NO_JOY

  while(this->running){
    InputState* state = NULL;
   
#ifndef NO_JOY 
    if(this->input_reader->getInputState(&state) < 0){
      fprintf(stderr, "In Server::run, unable to get valid input state from reader.\n");
      // ERROR HANDLING, Thinking init and continue
    }
#endif // #ifndef NO_JOY

#ifdef NO_JOY
    state = new InputState();

    // Modify according to stdin
    FD_ZERO(&readfd);
    FD_SET(STDIN, &readfd);

    if(select(STDIN+1, &readfd, NULL, NULL, &tv) == -1){
      perror("select");
      continue;
    }
    
    char input = 'o';
    if(FD_ISSET(STDIN, &readfd)){
      char inChar;
      if(read(STDIN, &inChar, sizeof(char)) == -1){
	perror("shit");
	break;
      }
      input = inChar;
    }
    
    if(input == 'w'){
      joy_y += 5;
      joy_y = constrain(joy_y, 0, 255);
    }else if(input == 's'){
      joy_y -= 5;
      joy_y = constrain(joy_y, 0, 255);
    }

    if(input == 'a'){
      joy_x -= 5;
      joy_x = constrain(joy_x, 0, 255);
    }else if(input == 'd'){
      joy_x += 5;
      joy_x = constrain(joy_x, 0, 255);
    }

    if(input == 'q'){
      joy_z -= 5;
      joy_z = constrain(joy_z, 0, 255);
    }else if(input == 'e'){
      joy_z += 5;
      joy_z = constrain(joy_z, 0, 255);
    }
    
    state->setValue(JOY_X, (unsigned char)joy_x);
    state->setValue(JOY_Y, (unsigned char)joy_y);
    state->setValue(JOY_TWI, (unsigned char)joy_z);
    state->setValue(JOY_BUT_0, (unsigned char)joy_but_0);
    state->setValue(JOY_BUT_1, (unsigned char)joy_but_1);
#endif // #ifdef NO_JOY

    unsigned char* buffer;
    state->toString(&buffer);
#ifdef DEBUG
    printf("//INPUTSTATE----------------------------------\n");
    print_name_values(buffer, InputState::getStringLength(), InputStateNames::names);
#endif // #ifdef DEBUG

#ifndef NO_SOCK
    if(this->port_comm->write_port(buffer, InputState::getStringLength()) == FAILURE_PORT){
      fprintf(stderr, "Bad\n");
    }
#endif // #ifndef NO_SOCK

    delete state;
    delete[] buffer;
    usleep(SERVER_DELAY_US);
  }
}

int Server::init(void){

  int errors = 0;

  if(init_sigs() < 0){
    fprintf(stderr, "In Server::init, init_sigs returned errors.\n");
    ++errors;
  }

#ifndef NO_JOY
  printf("Attempting to init input reader.\n");
  this->input_reader = new InputReader(this->fd_joystick, this->fd_ardui, this->baud);
  printf("Success at input reader.\n");
#endif // #ifndef NO_JOY
#ifndef NO_SOCK
  printf("Attempting socket connection.\n");
  this->port_comm = new PortComm("", this->portno, PortComm::SERVER);
  printf("Sucess at socket connection.\n");
#endif // #ifnedf NO_SOCK

  if(errors != 0){
    fprintf(stderr, "In Server::init, errors occurred during initialization.\n");
    return -1;
  }
  return 0;
}

int Server::init_sigs(void){
  // Interrupt Handler
  int errors = 0;
  if(signal(SIGINT, (__sighandler_t)int_sig_handle) == SIG_ERR){
    fprintf(stderr, "Unable to initialize interrupt handler.\n");
    ++errors;
  }
  
  // Sigpipe Handler
  if(signal(SIGPIPE, (__sighandler_t)pipe_sig_handle) == SIG_ERR){
    fprintf(stderr, "Unable to initialize sigpipe handler.\n");
    ++errors;
  }

  if(errors != 0){
    fprintf(stderr, "Unable to correctly initialize all signal handlers.\n");
    return -1;
  }
  return 0;
}

void Server::int_sig_handle(int sig){
  // Interrupt Caught
  char* input = new char[100];
  unsigned int nbytes = 100;

  printf("Interrupt Caught.\n");

  do{
    printf("Really Quit[Y/n]: ");
    getline(&input, &nbytes, stdin);
  }while(input[0] != 'Y' && input[0] != 'n' &&
	 !(input[0] == 'b' && input[1] == 'd'));

  if(input[0] == 'Y'){
    Server::theServer->running = false;
    exit(1);
  }else if(input[0] == 'n'){

  }else if(input[0] == 'b' && input[1] == 'd'){
    // Backdoor
    printf("BD\n");
    exit(1);
  }else{
    printf("I really fucked up.\n");
  }
  delete input;
}

void Server::pipe_sig_handle(int sig){
  printf("Sig pip received.\n");
  exit(1);
}

Server* Server::getServer(void){
  return theServer;
}

#endif
